/**
 * Copyright (c) 2019, Freqchip
 * 
 * All rights reserved.
 * 
 * 
 */

/*
 * INCLUDES
 */
#include "jump_table.h"
#include "co_printf.h"
#include "driver_plf.h"
#include "os_msg_q.h"
#include "gatt_api.h"

/* FreeRTOS headers */
#include "FreeRTOS.h"
#include "FreeRTOSConfig.h"
#include "task.h"
#include "timers.h"
#include "semphr.h"
#include "event_groups.h"

/*
 * MACROS
 */

/*
 * CONSTANTS
 */

/*
 * TYPEDEFS
 */

/*
 * GLOBAL VARIABLES
 */
 
/*
 * LOCAL VARIABLES
 */
static QueueHandle_t rtos_gatt_msg_queue;

/*
 * EXTERN FUNCTIONS
 */
void ble_stack_schedule(void);
void freertos_baseband_restore_done(void);

/*
 * PUBLIC FUNCTIONS
 */

#ifdef __GNUC__
__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __current_sp(void)
{
  uint32_t result;

  __ASM volatile ("MOV %0, sp" : "=r" (result) );
  return(result);
}
#endif  // _GNUC__

/*********************************************************************
 * @fn      rtos_gatt_msg_task
 *
 * @brief   Task entity to deal GATT messages generated by HOST layer.
 *
 * @param   arg - Parameters passed during task creation.
 *       
 * @return  None.
 */
static void rtos_gatt_msg_task(void *arg)
{
    os_event_t msg;
    
    while(1) {
        if(xQueueReceive(rtos_gatt_msg_queue, (void *)&msg, portMAX_DELAY) == pdPASS) {
            co_printf("rtos_gatt_msg_task\r\n");
            gatt_msg_default(&msg);
        }
    }
}

/*********************************************************************
 * @fn      app_rtos_entry
 *
 * @brief   RTOS relative entries. User should check FreeRTOSConfig.h for function
 *          implementations.
 *          RTOS_ENTRY_TYPE_INIT - used to initialize RTOS. Two tasks has to
 *              be created here, one is for ble stack (user_entry_after_ble_init
 *              will be called inside this task), one is for dealing GATT messages
 *              (gatt_msg_handler registed by gatt_add_service will be called inside
 *              this task). The ble stack task has to take the most high priority.
 *              We suggest that user create their own tasks in user_entry_after_ble_init
 *              function for safety.
 *          RTOS_ENTRY_TYPE_STACK_PUSH - used by lower layer to inform ble stack task
 *              that a new messge has been generated.
 *          RTOS_ENTRY_TYPE_STACK_YIELD - used by ble stack to pend for new message.
 *          RTOS_ENTRY_TYPE_WAKEUP_RESTORE - used to restore RTOS information after wake
 *              up, especially the OS tick information.
 *          RTOS_ENTRY_TYPE_POST_GATT_MSG - used by HOST layer to send GATT message
 *              to gatt_msg_task.
 *
 * @param   type    - @ref RTOS_ENTRY_TYPE_xx
 *          arg     - depend on entry type
 *       
 * @return  None.
 */
__attribute__((section("ram_code")))void app_rtos_entry(uint8_t type, void *arg)
{
    static TaskHandle_t ble_stack_task_handle;
    static TaskHandle_t ble_gatt_msg_task_handle;
    BaseType_t xHigherPriorityTaskWoken = pdFAIL;
    
    if(type == RTOS_ENTRY_TYPE_INIT) {
		uint32_t *task_handle;
		
        /* create ble stack task */
        xTaskCreate((TaskFunction_t)ble_stack_schedule, "BLE STACK", 512, NULL, ( configMAX_PRIORITIES - 1 ), &ble_stack_task_handle);
		task_handle = (uint32_t *)ble_stack_task_handle;
        co_printf("ble_stack_task_handle stack start: 0x%08x.\r\n", task_handle[12]);
		
        /* create ble stack task */
        xTaskCreate((TaskFunction_t)rtos_gatt_msg_task, "GATT MSG TASK", 512, NULL, ( configMAX_PRIORITIES - 2 ), &ble_gatt_msg_task_handle);
		task_handle = (uint32_t *)ble_gatt_msg_task_handle;
        co_printf("ble_gatt_msg_task_handle stack start: 0x%08x.\r\n", task_handle[12]);
		
        /* create test semaphore */
        rtos_gatt_msg_queue = xQueueCreate(10, sizeof(os_event_t));
        
        /* Initialize APP, has to be called after ble_stack_task_handle is created because ke_event_set will be called inside this function */
        appm_init();

        /* Start the scheduler. */
        vTaskStartScheduler();
    }
    else if(type == RTOS_ENTRY_TYPE_STACK_PUSH) {
        if((__get_PSP() == __current_sp())
            ||(xTaskGetSchedulerState() == taskSCHEDULER_NOT_STARTED)) {
            xTaskNotify(ble_stack_task_handle, 0, eSetValueWithOverwrite);
        }
        else {
            xTaskNotifyFromISR(ble_stack_task_handle, 0, eSetValueWithOverwrite, &xHigherPriorityTaskWoken);
            if(xHigherPriorityTaskWoken == pdTRUE) {
                taskYIELD();
            }
        }
    }
    else if(type == RTOS_ENTRY_TYPE_STACK_YIELD) {
        xTaskNotifyWait(0, 0, 0, portMAX_DELAY);
    }
    else if(type == RTOS_ENTRY_TYPE_WAKEUP_RESTORE) {
        freertos_baseband_restore_done();
    }
    else if(type == RTOS_ENTRY_TYPE_POST_GATT_MSG) {
        co_printf("RTOS_ENTRY_TYPE_POST_GATT_MSG\r\n");
        while(xQueueSend(rtos_gatt_msg_queue, arg, 0) != pdTRUE);
    }
}

